#include <iostream>
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <dynamic_reconfigure/server.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include "lidar_tool/param_extinctConfig.h"

using namespace std;

Eigen::Affine3f transform0;
Eigen::Affine3f transform1;
Eigen::Affine3f transform2;
Eigen::Affine3f transform3;
static void Callbackdynamic(lidar_tool::param_extinctConfig &config)
{
    transform0 = Eigen::Affine3f::Identity();
    transform0.translation() << config.x0, config.y0, config.z0;
    transform0.rotate(Eigen::AngleAxisf(config.yaw0, Eigen::Vector3f::UnitZ()));
    transform0.rotate(Eigen::AngleAxisf(config.pitch0, Eigen::Vector3f::UnitY()));
    transform0.rotate(Eigen::AngleAxisf(config.roll0, Eigen::Vector3f::UnitX()));

    std::cout << "ffff:\n"
              << transform0.matrix() << std::endl;

    transform1 = Eigen::Affine3f::Identity();
    transform1.translation() << config.x1, config.y1, config.z1;
    transform1.rotate(Eigen::AngleAxisf(config.yaw1, Eigen::Vector3f::UnitZ()));
    transform1.rotate(Eigen::AngleAxisf(config.pitch1, Eigen::Vector3f::UnitY()));
    transform1.rotate(Eigen::AngleAxisf(config.roll1, Eigen::Vector3f::UnitX()));

    transform2 = Eigen::Affine3f::Identity();
    transform2.translation() << config.x2, config.y2, config.z2;
    transform2.rotate(Eigen::AngleAxisf(config.yaw2, Eigen::Vector3f::UnitZ()));
    transform2.rotate(Eigen::AngleAxisf(config.pitch2, Eigen::Vector3f::UnitY()));
    transform2.rotate(Eigen::AngleAxisf(config.roll2, Eigen::Vector3f::UnitX()));

    transform3 = Eigen::Affine3f::Identity();
    transform3.translation() << config.x3, config.y3, config.z3;
    transform3.rotate(Eigen::AngleAxisf(config.yaw3, Eigen::Vector3f::UnitZ()));
    transform3.rotate(Eigen::AngleAxisf(config.pitch3, Eigen::Vector3f::UnitY()));
    transform3.rotate(Eigen::AngleAxisf(config.roll3, Eigen::Vector3f::UnitX()));
}

ros::NodeHandle &get_handle()
{
    static ros::NodeHandle nh;
    return nh;
}
template <class T>
ros::Publisher &get_publisher(const std::string &topic)
{
    static std::map<std::string, ros::Publisher> publisher_map;
    if (publisher_map.find(topic) == publisher_map.end())
    {
        publisher_map[topic] = get_handle().advertise<T>(topic, 1000);
    }
    return publisher_map[topic];
}

template <typename Point>
void pubpc(std::string topic, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    get_publisher<sensor_msgs::PointCloud2>(topic).publish(msgs);
}

pcl::PointCloud<pcl::PointXYZI>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud3(new pcl::PointCloud<pcl::PointXYZI>);

int loadData(const char *file, void **data, unsigned int *length)
{
    std::fstream dataFile(file, std::ifstream::in);

    if (!dataFile.is_open())
    {
        std::cout << "Can't open files: " << file << std::endl;
        return -1;
    }

    // get length of file:
    unsigned int len = 0;
    dataFile.seekg(0, dataFile.end);
    len = dataFile.tellg();
    dataFile.seekg(0, dataFile.beg);

    // allocate memory:
    char *buffer = new char[len];
    if (buffer == NULL)
    {
        std::cout << "Can't malloc buffer." << std::endl;
        dataFile.close();
        exit(-1);
    }

    // read data as a block:
    dataFile.read(buffer, len);
    dataFile.close();

    *data = (void *)buffer;
    *length = len;
    return 0;
}

int main(int argc, char *argv[])
{

    // load points cloud
    //  注意，bin文件存储格式为float32,如果其他类型读取数据不对。
    std::string dataFile = "/home/zwh/ws/Adam/EasyPointPillars/data/kitti/testing/velodyne/000099.bin";
    unsigned int length = 0;
    void *data = NULL;
    std::shared_ptr<char> buffer((char *)data, std::default_delete<char[]>());
    loadData(dataFile.data(), &data, &length);
    buffer.reset((char *)data);

    float *points = (float *)buffer.get();
    size_t points_size = length / sizeof(float) / 4;

    pcl::PointCloud<pcl::PointXYZI> pc;
    for (size_t i = 0; i < points_size; i++)
    {
        pcl::PointXYZI p;
        p.x = points[4 * i];
        p.y = points[4 * i + 1];
        p.z = points[4 * i + 2];
        p.intensity = points[4 * i + 3];
        pc.push_back(p);
    }

    // if (pcl::io::loadPCDFile<pcl::PointXYZI>("/home/zwh/work_space/18xx/lidar_tool_ws/src/lidar_tool/4lidarpcd/front.pcd", *cloud0) == -1 ||
    //     pcl::io::loadPCDFile<pcl::PointXYZI>("/home/zwh/work_space/18xx/lidar_tool_ws/src/lidar_tool/4lidarpcd/back.pcd", *cloud1) == -1 ||
    //     pcl::io::loadPCDFile<pcl::PointXYZI>("/home/zwh/work_space/18xx/lidar_tool_ws/src/lidar_tool/4lidarpcd/left.pcd", *cloud2) == -1 ||
    //     pcl::io::loadPCDFile<pcl::PointXYZI>("/home/zwh/work_space/18xx/lidar_tool_ws/src/lidar_tool/4lidarpcd/right.pcd", *cloud3) == -1) //*Žò¿ªµãÔÆÎÄŒþ
    // {
    //     PCL_ERROR("Couldn't read file.pcd\n");
    //     return (-1);
    // }

    ros::init(argc, argv, "test_node");
    dynamic_reconfigure::Server<lidar_tool::param_extinctConfig> server;
    dynamic_reconfigure::Server<lidar_tool::param_extinctConfig>::CallbackType f;
    f = boost::bind(&Callbackdynamic, _1); //绑定回调函数
    server.setCallback(f);

    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        pcl::PointCloud<pcl::PointXYZI> temp;

        pcl::transformPointCloud(pc, temp, transform0);

        pubpc("/pointcloud_Pandar640", temp);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
